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Abstract. This paper is concerned with the application of the theory of quasivelocities for 
optimal control for underactuated mechanical systems. Using this theory, we convert the 
original problem in a variational second-order lagrangian system subjected to constraints. The 
■ equations of motion are geometrically derived using an adaptation of the classical Skinner and 
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1. Introduction 

1^ '■ The mathematical activity in the last century in dynamical systems, mechanics and related 

areas has been recently extended to the control and optimal control of mechanical systems. In 
particular, there are an increasing interest in the control of underactuated mechanical systems 
(see [21 [5] and references therein). These type of mechanical systems are characterized by the 
^2 ' fact that there are more degrees of freedom than actuators, being their qualitative behavior quite 

different than fully actuated control systems. 

Geometrically, quasivelocities are the components of velocities, describing a mechanical system, 
relative to a set of vector fields (in principle, local) that span on each point the fibers of the 
^ ' tangent bundle of the configuration space. The main point is that these vector fields need not 

. be associated with (local) configuration coordinates on the configuration space. In this paper 

we will use quasivelocities as a tool for describing optimal control problem for underactuated 
mechanical systems. 

The paper is organized as follows. In Section 2 we describe the concept of quasivelocities 
' from a geometric point of view and we introduce the Euler-Lagrange equations on quasivelocities 

, (called Hamel equations). In section 3 we describe the conditions of optimality for optimal control 

problem using the Skinner and Rusk formulation |10| [1] . 



2. Quasivelocities 

Let Q be a 71 dimensional differentiable manifold, and L : TQ — > M a Lagrangian function 
determining the dynamics. Let (g'^), 1 < A < n, be local coordinates on Q and choose a local 
basis of vector fields Xb with 1 < B < n, defined in the same coordinate neighborhood. The 
components oiXs relative to the standard basis ^ will be denoted X^, that is Xb = X^{q)^p^ . 

Let (y^, y") (the quasivelocities) be the components of a velocity vector v on TQ relative 
to the basis Xb, then 



V = y^XB{q)^y^X^{q)^. 

Therefore, = y^Xg(q), then 



L{qA)^L{q,y''X^{q)) :-/(q,y). 

On TQ we have induced coordinates {{q^,y^) | A = 1, ...,n}. 

The Lie bracket of the vector fields Xa is [Xyi,^^] — G^^Xd, where G^g are called Hamel's 
transpositional symbols or structure coefficients. 
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Given a Lagrangian function L : TQ — M, the Euler- Lagrange equations in quasivelocities or 
Hamel equations are 

q^ = y^Xiiq) 

d ( dl\ _ dl . a , B 
dt\dy^) " dqB^B ^^^y dyD 

These equations were introduced by [7] (see also [S] ) . It is interesting to note that these equations 
admit a nice, useful and intrinsic interpretation in terms of mechanics on Lie algebroids (see [8]. 



3. Optimal Control for Underactuated Mechanical Systems 

We recall that a Lagrangian Control System is underactuated if the numbers of the control 
inputs is less than the dimension of the configuration space. We assume, in the sequel, that the 
system is controllable [2]. 

Consider a lagrangian function L : TQ — > M. Adding external forces and controlled forces we 
have that the equations of motion are: 

d f dL\ dL ^ —a 

where F = F^{q,q)dq"^ represents given external forces and — X^(q)(ig", 1 < a < m < n, 
the control forces. 

Complete with independent 1-forms X" to obtain a local basis {X°',X°'} of A^Q and take 
its dual basis that we denote by {Xa,Xa}. Now, considering the quasivelocities induced by the 
local basis {Xa,Xa}, the control equations are written as 

q^ ^ y^X^{q) 

where 1 < a < m, m + 1 < a < n, and u(t) = (ui(i), Umit)) G U where U is and open subset 
of R™ containing 0. 

For solving an optimal control problem we need to find a trajectory ((7^(t), u"(t)) (called an 
optimal curve) of the configuration variables and control inputs satisfying the control equations 
from given initial and final conditions: (g^(fo), 2/'^(io))j W^^S^-, 2/^(*/)) and minimizing the cost 
functional 

A= r Ciq^{t),y^it),u'^{t))dt. 
Jto 

For other hand, a second order variational Lagrangian problem with constraints is given by 

rT 

min / L{q^,q^,q^) dt 
?(•) Jo 

subject to the constraints 

m.q^,q^,q^) = 0. 

In the sequel we will show the equivalence of both theories (optimal control for underactuated 
systems and second order variational problems with constraints) under some regularity conditions 
(see [2] and references therein). Indeed, our initial optimal control problem is equivalent to the 
following constrained variational problem 
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' dq^ 
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dl 


-( 

dt \ 




~ d^' 



tf 

to 



Minimize yi= / L {q'^{t),y'^{t),y'^{t)) dt 
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subject to constraints 

and where L is defined as 

More, geometrically, we have that (g^,y"*,y^) are coordinates on T'^'^'^Q (the second order 
tangent bundle) and the constraints determine a submanifold M of T'-^^Q and L : T^^^Q — > M. 
The canonical immersion j2 : T^^^Q — > T{TQ) in the induced coordinates iq^,y'^,y'^) is 

Assume that the matrix ( a ^ i „ ) is regular, then we can rewrite the constraints 

in the form y^ — {q^ , y^ , y°') and select coordinates {q^,y^,y'^) on M. 
Hence,(j2)|M :M^T(Tg) is 

M ^'^4" TTQ 
{q^.y^^r) ^ {q^.y^-.XiyB^y^G'^iq^.y^^r)) 

Let us define Lm by Lm — L |m: 3VC — > M and consider Wo = M xtq T*TQ with induced 
coordinates {q^,y^,y'',pA,PA)- 

Now, we will describe geometrically the problem based on the Skinner and Rusk formalism 
(see [10]). 



Wo = Mxtq T*{TQ) 




Figure 1. Skinner and Rusk Formalism 

Let us define the 2- form Q = pr2{uJTQ) on Wq, where utq is the canonical symplcctic form 
on T*TQ, and H{vj;,aj;) = (a^, (J2) |m {vx)) - Lmivx) where a; G TQ,Vx G Mo, n (ttq Im)^^^;) 
and ax G T*TQ. 

In coordinates 

= dq'^ A + dy^ A dp^, 

H^PAX^{q)y^ +Par +Po.G''{q^,y^,r) -LM{q^,y^,r)- 

The intrinsic expression of this constrained problem is given by the following presymplectic 
equation 

ix^ = dH. (3.1) 

d 

Observe that /ceril = span {jrr^)- 
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Following the Gotay-Nestcr-Hinds algorithm 6 for presymplectic hamiltonian systems we 
obtain the primary constraints dH = 0, that is 

Therefore the dynamics is restricted to the manifold Wi determined by the vanishing of the 
constraints = 0. Observe that dimM^i = An with induced coordinates {q^ ,y'^ ,PA^Pa)■ 
A curve t {q^{t),y^{t),y°'{t),pA{t),PA{t}) solution of the equations (|3.ip must verify the 
following system of differential-algebraic equations. 



X^iq{t))y^it) (3.2) 



IT 

^ = G"(g-4(t),/(i),^(t)), ^-nt) (3.3) 

^ - ~Pc{t)^{my''{t)-Pa{t)^{q''{tu''m'') 

+^iq''{t),y^{t),y') (3.4) 



^ = -pa{t)X^{q{t))~p^{t)^{q^{t),y^{t),y') 



+^iqB^t),y^^t),y^) (3.5) 



Pa{t) - ~Pa{t)^{q^it),y^{t),y'>) + ^{q''{t),y^{t),y'>) (3.6) 
From Equations (13. Sp and p.6p we deduce 



Differentiating with respect to time, replacing in the previous equality and using p.4p . we 
obtain the following system of equations 

d ( OLm ~ dG" \ 



dLM 


^ dG"\ 


dy<^ 


dy'^ ) 


( dL-M 


^ dG" 


\ dqA 


dqA 



dy" I 

^ dqD dqD 



(3.8) 



Let us consider the 2- form Qwi ~ *iVi^ where '■ Wi ^ Wq is the canonical inclusion. 

Theorem 3.1. The submanifold {Wi,flwi) is symplectic if and only if for any system of local 
coordinates {q^ ,y^ ,y°- ,pa,Pa) on Wq 

Cd^L d^G" \ 

drW-P^Q^b] along Wi. 

/ mxm 

Proof: Let us recall that ilwi is symplectic of and only if T^Wi Ci (T^Wi)'^ = Vx e Wi, 
where 

(T^Wi)^ ^{ve T,Wo I ^wMh'M = O,for ah w £ T^T^i} . 
Suppose that (Wi, rJvKi) is symplectic and that 

\°-'Rah{x) = for some A° e M and a; G VKi . 
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Since 



dy" 



, 



then, A'' g| 



G TxWi but it is also in T^W^. This imphes that Ah = for all h and that the 
matrix {Hab) is regular. 

Now, suppose that the matrix {"Rab) is regular. As we have observed 



d(pa{x) 



_d_ 



and therefore, 

' dy'' 



^ TxWi and, in consequence, 

d_ 



r^VKi ® span <; ^ 



^abix) 



Now, let Z G T^Wi n (T^Wi) with x eWi. It follows that 

9 



= iz^Woix) 



, for all a and izf^Wo = 0, for ah Z e T^Wi 



Then, Z G heiQwoix). This implies that 



dy'' 



Since Z e T^Wi then 



^ = A. 



= dipa{x)(Z) = dipa{x) ( Afc ^ 



and, consequently, A& = 0, for all b, and Z = 0. □ 
Under the hypothesis of Theorem 13.11 wc can rewrite the necessary conditions for optimality 
as an explicit system of differential equations where Equation p.Sp is replaced by 



^ \ Q T y J 



dt ' dt2 



,PA,Pa 



(3.9) 



3.1. Example. [The Planar Rigid Body] 

The configuration space for this system in Q = x 5'^ and it can be considered as the 
simplest example in the category of rigid body dynamics. The three degrees of freedom describe 
the translations in and the rotation about its center of mass. The configuration is given by the 
followings variables: 9 describes the relative orientation the body reference frame with respect to 
the inertial reference frame. The vector (x, y) denotes the position of the center of mass measured 
with respect to the inertial reference frame. The lagrangian is of kinetical type 

L = ^q'^S{q)q, where g(g) = 

and where m is the mass of the body and J is its moment of inertia about the center of mass. 
If we assume that the body moves in a plane perpendicular to the direction of the gravitational 
forces being the potential energy zero. For the planar body, the control forces that we consider 
are applied to a point on the body with distance h > from the center of mass, along the body 
a;— axis (see ;3: for more details about this example). 

The equations of motion are 

mx — ui cos 6 — U2 sin 9 

my ~ ui sin 9 + U2 cos 9 

J 9 = —hu2 
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The control fields are 



X2 



COS0 d smO d 
+ ■ 



m dx m dy 
sin 6 d cos 6 d h d 

m dx m dy J 80^ 



and we complete the basis of vector fields with X3 = hsinO-^ — hcosO-^ — ^ 



The nonzero structure functions are 

h 



m/i2 + J 



— 



3 

^2 (m/i2 + J) J 



1 _ _rnh^±J_ __pi 

^23 ~ J ~ ^32^ 



e 



J 



13 



mh^ + J 



p3 

— "-21 



_p2 
*-31 



e3 
13 — ~" 



h 



p3 

— 



mh? + J 

Taking the corresponding quasivelocities {y^, 2/^, ?/^}, we have that 

,1' 



cosy 

X = y- y 



m 



o sin . 

h y h sm ( 

m 



y = y 



1 sin ^ 2 cos 9 3 



m 

= -y'j-y'- 



m 



— yh cos 9 



The Lagrangian of this system is 
l{x,y,9,y\y\y^) = ^ 



m mJ 



then the Hamel equations with controls are: 



"1 = y' + -Ay'f - hmiy'r + 



U2 



J' 



-y y 



-J — y — -jy y -y y 



= {J + mh'')f + —yY + hy'y^. 



Consider the following cost functional 



A 



1 



I) dt. 



Following our formalism this optimal control problem is equivalent to the constrained second- 
order variational problem determined by: 

A= / L{x,y,9,y\y^,y^,y\f,f)dt 
Jo 

and the second-order constraint 



/j2 

^{x, y, 9, y\y\y^ y\f,f) = {J + mh^)f + —yY + hy^y^ = , 



where 



L{x,y,9,y\y',y',y\r,r) = 



+ 



J - mhr 



y^+'j(yy-hm{y^f+ J 

— J — y - -jy y - y y 



-yV 
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Now, we rewrite the second-order constraint in the form 



y 



,yV 



yy^ 



Take now Wq — 'MxT*(T{R'^xS^)) with coordinates (x, y, 9, y^,y^,y^, y^,!/"^ ,pl,p2,P3,Pl,P2,P3)■ 
Now, the presymplectic 2-form fl, the Hamiltonian H and the primary constraints, ipi,ip2, 



are: 



SI = dx A dpi + dy A dp2 + dO A dps + dy^ A dpi + dy^ A dp2 + dy^ A dps 



H 



<P2 



Pi 

-P3 
1 

~2 
dH 

w 

P2 - 



TO 

.h 



TO 



y-^hsm6 



P2 



y'-j+y' 



+ piy^ +p2y^ -P3 



I sm fc 

TO 



y hcosO 



{J + mh^)J 



yV 



(J + nih^) 



y' + j{y'? - hmiy^)' + 



J — mK^ 



J 



Pi 



y'+'j{y^f-hMyy + 



-y^y^ 



J — mh^ 



1 
2 

2„,3 



J 



-y y 



J 

= 0, 



yV 



h ^ n IS 

-jy y -y y 



J + mh"^ 
J 



J + mh^ . 2 



J 



y 



J 



12 ^ 

y y -y y 



Pi = y^ + -j{y^f - hm^y'^f + 



J — mh^ 



2„,3 



J 



-y y 



P2 = 



J + to/i^ 

J 



J + to/i^ 



J 



/i 1 2 1 

- j2/ y -y y 



These constraints determine the submanifold Wi. Applying the Theorem 13. II we deduce that 
the 2-form Vlwn restriction of SI to Wi, is symplectic since 

3^11 3^12 A / 1 
3?2i 3^22 y \0 iJ + mh^)/J 

is regular. 

Therefore, the algorithm stabilizes in the first constraint submanifold Wi. Moreover, there 
exists a unique solution of the dynamics, a vector field X which satisfies ix^Wi — dHwi- In 
consequence, we have a unique control input which extremizes the objective function A. If we take 
the flow Ft : W\ W\ of the vector field X then we have that F^^Wx — ^Wi j then the evolution 
is symplectic preserving. Obviously, the hamiltonian function il\y^ is preserved by the solution 

of the optimal control problem, that is ^|^^ ° Ft = H\y^^. Both properties, symplecticity and 
preservation of energy, are important geometric invariants. In ^4,, we construct, using discrete 
variational calculus, numerical integrators which inherit some of the geometric properties of 
the optimal control problem (symplecticity, momentum preservation and a very good energy 
behavior). 



References 

[1] M. Barbero-Linan, A.Echeverria Enn'quez, D. Martin de Diego, M.C Munoz-Lccanda and N. Roman-Roy. 
Skinner-Rusk unified formalism for optimal control systems and applications. J. Phys. A; Math Theor. 40 
(2007), 12071-12093. 

[2] A.M. Bloch: N onholonomic Mechanics and Control, Interdisciplinary Applied Mathematics Series, 24, 

Springer- Verlag, New York (2003). 
[3] F. BuUo, A.D. Lewis. Geometric control of mechanical systems: Modeling, Analysis, and Design for Simple 

Mechanical Control Systems. Texts in Applied Mathematics, Springer Verlaeng, New York (2005). 
[4] L. Colombo, D. Martin de Diego and M. Zuccalli. Optimal Control for Underactuated Mechanical Systems: 

A Geometrical Approach. Journal Mathematical Physics (2010) (Acept.) 



8 



L. COLOMBO AND D. MARTIN DE DIEGO 



[5] J. Cortes. Geometric Control and Numerical Aspects of Nonholonomic Systems, Lec. Notes in Math., 1793, 

Springer- Vcrlag, Berlin (2002). 
[6] M. Gotay, J. Nestcr. Presymplectic Lagrangian systems I. The constraint algorithm and the equivalence 

theorem. Arm. I.H.P Pliys. Theor. 30, 129-142 (1979). 
[7] G. Haiucl. Die Lagraiige-Eulersche Gleichungen der Mechanik, Z. Math. Phys. 50, 1-57 (1904). 
[8] M. de Leon, J.C. Marrero, E. Martinez. Lagrangian submanifolds and dynamics on Lie algebroids, J. Phys. 

A: Math. Gen. 38 (2005), R241-R308. 
[9] J. I. Neirnark, N. A. Pufaev. Dynamics of Nonholonomic Systems. Translations of Mathematical Monographs, 

AMS, 33 (1972). 

[10] R. Skinner, R. Rusk. Generalized Hamiltonian dynamics L Formulation on T*Q © TQ. J. Math. Phys. 24 
(11) (1983), 2589-2594. 

L. Colombo; Instituto db Ciencias Matematicas (CSIC-UAM-UC3M-UCM), Serrano 123, 28006 Madrid, 
Spain and Departamento de Matematica, Universidad Nacional de La Plata, Calle 50 y 115, La Plata, 
Buenos Aires, Argentina 

E-mail address: leonardocolombo8670gmall . com 

D. Martin de Diego; Instituto de Ciencias Matematicas (CSIC-UAM-UC3M-UCM), Serrano 123, 
28006 Madrid, Spain 

E-mail address: david.martinaicmat.es 



